/*
 * Sgp4unit.cpp
 *
 *  Created on: 25/04/2012
 *      Author: ubuntu
 */

#include "sgp4unit.h"
#include "sgp4_math.h"
#include "sgp4_date.h"


/**
 * @brief Construye un nuevo objeto Sgp4unit.
 */
Sgp4unit::Sgp4unit() {
	this->satrec = new elsetrec::elsetrec();
}

/**
 * @brief Devuelve el objeto elsetrec que contiene sgp4unit
 * @return *elsetrec. Objeto elsetrec que contiene sgp4unit
 */
elsetrec* Sgp4unit::getSatrec(){
	return satrec;
}

/**
 * @brief esta función aporta información sobre puntos en movimiento del espacio profundo debido a la resonancia geopotencial en un día y medio.
 * @param cosim parámetro de entrada.
 * @param emsq parámetro de entrada.
 * @param s1 parámetro de entrada.
 * @param s2 parámetro de entrada.
 * @param s3 parámetro de entrada.
 * @param s4 parámetro de entrada.
 * @param s5 parámetro de entrada.
 * @param sinim parámetro de entrada.
 * @param ss1 parámetro de entrada.
 * @param ss2 parámetro de entrada.
 * @param ss3 parámetro de entrada.
 * @param ss4 parámetro de entrada.
 * @param ss5 parámetro de entrada.
 * @param sz1 parámetro de entrada.
 * @param sz3 parámetro de entrada.
 * @param sz11 parámetro de entrada.
 * @param sz13 parámetro de entrada.
 * @param sz21 parámetro de entrada.
 * @param sz23 parámetro de entrada.
 * @param sz31 parámetro de entrada.
 * @param sz33 parámetro de entrada.
 * @param tc parámetro de entrada.
 * @param xpidot parámetro de entrada.
 * @param z1 parámetro de entrada.
 * @param z3 parámetro de entrada.
 * @param z11 parámetro de entrada.
 * @param z13 parámetro de entrada.
 * @param z21 parámetro de entrada.
 * @param z23 parámetro de entrada.
 * @param z31 parámetro de entrada.
 * @param z33 parámetro de entrada.
 * @param eccsq parámetro de entrada.
 * @param em parámetro de entrada / salida.
 * @param argpm parámetro de entrada / salida.
 * @param inclm parámetro de entrada / salida.
 * @param mm parámetro de entrada / salida.
 * @param nm parámetro de entrada / salida.
 * @param nodem parámetro de entrada / salida.
 * @param dndt parámetro de entrada / salida.
 */
void Sgp4unit::dsinit
     (
       gravconsttype whichconst,
       double cosim,  double emsq,   double s1,     double s2,
       double s3,     double s4,     double s5,      double sinim,  double ss1,
       double ss2,    double ss3,    double ss4,     double ss5,    double sz1,
       double sz3,    double sz11,   double sz13,    double sz21,   double sz23,
       double sz31,   double sz33,   double tc,      double xpidot, double z1,
       double z3,      double z11,    double z13,
       double z21,    double z23,    double z31,     double z33,    double eccsq,
       double& em,    double& argpm,  double& inclm, double& mm,
       double& nm,    double& nodem, double& dndt
     )
{

     /* --------------------- local variables ------------------------ */
     const double twopi = 2.0 * pi;

     double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221  , f311  ,
          f321  , f322  , f330  , f441  , f442  , f522  , f523  ,
          f542  , f543  , g200  , g201  , g211  , g300  , g310  ,
          g322  , g410  , g422  , g520  , g521  , g532  , g533  ,
          ses   , sgs   , sghl  , sghs  , shs   , shll  , sis   ,
          sini2 , sls   , temp  , temp1 , theta , xno2  , q22   ,
          q31   , q33   , root22, root44, root54, rptim , root32,
          root52, x2o3  , xke   , znl   , emo   , zns   , emsqo,
          tumin, mu, radiusearthkm, j2, j3, j4, j3oj2;

     q22    = 1.7891679e-6;
     q31    = 2.1460748e-6;
     q33    = 2.2123015e-7;
     root22 = 1.7891679e-6;
     root44 = 7.3636953e-9;
     root54 = 2.1765803e-9;
     rptim  = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec
     root32 = 3.7393792e-7;
     root52 = 1.1428639e-7;
     x2o3   = 2.0 / 3.0;
     znl    = 1.5835218e-4;
     zns    = 1.19459e-5;

     // sgp4fix identify constants and allow alternate values
     getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );

     /* -------------------- deep space initialization ------------ */
     satrec->setIrez(0);
     if ((nm < 0.0052359877) && (nm > 0.0034906585))
    	 satrec->setIrez(1);
     if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5))
    	 satrec->setIrez(2);

     /* ------------------------ do solar terms ------------------- */
     ses  =  ss1 * zns * ss5;
     sis  =  ss2 * zns * (sz11 + sz13);
     sls  = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq);
     sghs =  ss4 * zns * (sz31 + sz33 - 6.0);
     shs  = -zns * ss2 * (sz21 + sz23);
     // sgp4fix for 180 deg incl
     if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2))
       shs = 0.0;
     if (sinim != 0.0)
       shs = shs / sinim;
     sgs  = sghs - cosim * shs;

     /* ------------------------- do lunar terms ------------------ */
     satrec->setDedt(ses + s1 * znl * s5);
     satrec->setDidt(sis + s2 * znl * (z11 + z13));
     satrec->setDmdt(sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq));
     sghl = s4 * znl * (z31 + z33 - 6.0);
     shll = -znl * s2 * (z21 + z23);
     // sgp4fix for 180 deg incl
     if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2))
         shll = 0.0;
     satrec->setDomdt(sgs + sghl);
     satrec->setDnodt(shs);
     if (sinim != 0.0)
       {
    	 satrec->setDomdt(satrec->getDomdt() - cosim / sinim * shll);
    	 satrec->setDnodt(satrec->getDomdt() + shll / sinim);
       }

     /* ----------- calculate deep space resonance effects -------- */
     dndt   = 0.0;
     theta  = fmod(satrec->getGsto() + tc * rptim, twopi);
     em     = em + satrec->getDedt() * satrec->getT();
     inclm  = inclm + satrec->getDidt() * satrec->getT();
     argpm  = argpm + satrec->getDomdt() * satrec->getT();
     nodem  = nodem + satrec->getDnodt() * satrec->getT();
     mm     = mm + satrec->getDmdt() * satrec->getT();
     //   sgp4fix for negative inclinations
     //   the following if statement should be commented out
     //if (inclm < 0.0)
     //  {
     //    inclm  = -inclm;
     //    argpm  = argpm - pi;
     //    nodem = nodem + pi;
     //  }

     /* -------------- initialize the resonance terms ------------- */
     if (satrec->getIrez() != 0)
       {
         aonv = pow(nm / xke, x2o3);

         /* ---------- geopotential resonance for 12 hour orbits ------ */
         if (satrec->getIrez() == 2)
           {
             cosisq = cosim * cosim;
             emo    = em;
             em     = satrec->getEcco();
             emsqo  = emsq;
             emsq   = eccsq;
             eoc    = em * emsq;
             g201   = -0.306 - (em - 0.64) * 0.440;

             if (em <= 0.65)
               {
                 g211 =    3.616  -  13.2470 * em +  16.2900 * emsq;
                 g310 =  -19.302  + 117.3900 * em - 228.4190 * emsq +  156.5910 * eoc;
                 g322 =  -18.9068 + 109.7927 * em - 214.6334 * emsq +  146.5816 * eoc;
                 g410 =  -41.122  + 242.6940 * em - 471.0940 * emsq +  313.9530 * eoc;
                 g422 = -146.407  + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc;
                 g520 = -532.114  + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc;
               }
               else
               {
                 g211 =   -72.099 +   331.819 * em -   508.738 * emsq +   266.724 * eoc;
                 g310 =  -346.844 +  1582.851 * em -  2415.925 * emsq +  1246.113 * eoc;
                 g322 =  -342.585 +  1554.908 * em -  2366.899 * emsq +  1215.972 * eoc;
                 g410 = -1052.797 +  4758.686 * em -  7193.992 * emsq +  3651.957 * eoc;
                 g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc;
                 if (em > 0.715)
                     g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc;
                   else
                     g520 = 1464.74 -  4664.75 * em +  3763.64 * emsq;
               }
             if (em < 0.7)
               {
                 g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21  * eoc;
                 g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc;
                 g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4  * eoc;
               }
               else
               {
                 g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc;
                 g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc;
                 g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc;
               }

             sini2=  sinim * sinim;
             f220 =  0.75 * (1.0 + 2.0 * cosim+cosisq);
             f221 =  1.5 * sini2;
             f321 =  1.875 * sinim  *  (1.0 - 2.0 * cosim - 3.0 * cosisq);
             f322 = -1.875 * sinim  *  (1.0 + 2.0 * cosim - 3.0 * cosisq);
             f441 = 35.0 * sini2 * f220;
             f442 = 39.3750 * sini2 * sini2;
             f522 =  9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) +
                     0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) );
             f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim +
                    10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq));
             f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq *
                    (-12.0 + 8.0 * cosim + 10.0 * cosisq));
             f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq *
                    (12.0 + 8.0 * cosim - 10.0 * cosisq));
             xno2  =  nm * nm;
             ainv2 =  aonv * aonv;
             temp1 =  3.0 * xno2 * ainv2;
             temp  =  temp1 * root22;
             satrec->setD2201(temp * f220 * g201);
             satrec->setD2211(temp * f221 * g211);
             temp1 =  temp1 * aonv;
             temp  =  temp1 * root32;
             satrec->setD3210(temp * f321 * g310);
             satrec->setD3222(temp * f322 * g322);
             temp1 =  temp1 * aonv;
             temp  =  2.0 * temp1 * root44;
             satrec->setD4410(temp * f441 * g410);
             satrec->setD4422(temp * f442 * g422);
             temp1 =  temp1 * aonv;
             temp  =  temp1 * root52;
             satrec->setD5220(temp * f522 * g520);
             satrec->setD5232(temp * f523 * g532);
             temp  =  2.0 * temp1 * root54;
             satrec->setD5421(temp * f542 * g521);
             satrec->setD5433(temp * f543 * g533);
             satrec->setXlamo(fmod(satrec->getMo() + satrec->getNodeo() + satrec->getNodeo()-theta - theta, twopi));
             satrec->setXfact(satrec->getMdot() + satrec->getDmdt() + 2.0 * (satrec->getNodedot() + satrec->getDnodt() - rptim) - satrec->getNo());
             em    = emo;
             emsq  = emsqo;
           }

         /* ---------------- synchronous resonance terms -------------- */
         if (satrec->getIrez() == 1)
           {
             g200  = 1.0 + emsq * (-2.5 + 0.8125 * emsq);
             g310  = 1.0 + 2.0 * emsq;
             g300  = 1.0 + emsq * (-6.0 + 6.60937 * emsq);
             f220  = 0.75 * (1.0 + cosim) * (1.0 + cosim);
             f311  = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim);
             f330  = 1.0 + cosim;
             f330  = 1.875 * f330 * f330 * f330;
             satrec->setDel1(3.0 * nm * nm * aonv * aonv);
             satrec->setDel2(2.0 * satrec->getDel1() * f220 * g200 * q22);
             satrec->setDel3(3.0 * satrec->getDel1() * f330 * g300 * q33 * aonv);
             satrec->setDel1(satrec->getDel1() * f311 * g310 * q31 * aonv);
             satrec->setXlamo(fmod(satrec->getMo() + satrec->getNodeo() + satrec->getArgpo() - theta, twopi));
             satrec->setXfact(satrec->getMdot() + xpidot - rptim + satrec->getDmdt() + satrec->getDomdt() + satrec->getDnodt() - satrec->getNo());
           }

         /* ------------ for sgp4, initialize the integrator ---------- */
         satrec->setXli(satrec->getXlamo());
         satrec->setXni(satrec->getNo());
         satrec->setAtime(0.0);
         nm    = satrec->getNo() + dndt;
       }

//#include "debug3.cpp"
}  // end dsinit

/**
 * @brief Esta función aporta el periodo de una larga duración de los típicos elementos del espacio porfundo.
 * @param inclo parametro de entrada.
 * @param init parametro de entrada.
 * @param ep parametro de entrada / salida.
 * @param inclp parametro de entrada / salida.
 * @param nodep parametro de entrada / salida.
 * @param argpp parametro de entrada / salida.
 * @param mp parametro de entrada / salida.
 */
void Sgp4unit::dpper
(

  double inclo,
  char init,
  double& ep,    double& inclp, double& nodep,  double& argpp, double& mp
)
{
	/*
	 * (
                                inclm, satrec->init,
                                satrec->ecco, satrec->inclo, satrec->nodeo, satrec->argpo, satrec->mo,

                              );
                               dpper
             (
               satrec->inclo,
               'n', ep, xincp, nodep, argpp, mp
             );
	 */
/* --------------------- local variables ------------------------ */
const double twopi = 2.0 * pi;
double alfdp, betdp, cosip, cosop, dalf, dbet, dls,
     f2,    f3,    pe,    pgh,   ph,   pinc, pl ,
     sel,   ses,   sghl,  sghs,  shll, shs,  sil,
     sinip, sinop, sinzf, sis,   sll,  sls,  xls,
     xnoh,  zf,    zm,    zel,   zes,  znl,  zns;

/* ---------------------- constants ----------------------------- */
zns   = 1.19459e-5;
zes   = 0.01675;
znl   = 1.5835218e-4;
zel   = 0.05490;

/* --------------- calculate time varying periodics ----------- */
zm    = satrec->getZmos() + zns * satrec->getT();
// be sure that the initial call has time set to zero
if (init == 'y')
    zm = satrec->getZmos();
zf    = zm + 2.0 * zes * sin(zm);
sinzf = sin(zf);
f2    =  0.5 * sinzf * sinzf - 0.25;
f3    = -0.5 * sinzf * cos(zf);
ses   = satrec->getSe2()* f2 + satrec->getSe3() * f3;
sis   = satrec->getSi2() * f2 + satrec->getSi3() * f3;
sls   = satrec->getSl2() * f2 + satrec->getSl3() * f3 + satrec->getSl4() * sinzf;
sghs  = satrec->getSgh2() * f2 + satrec->getSgh3() * f3 + satrec->getSgh4() * sinzf;
shs   = satrec->getSh2() * f2 + satrec->getSh3() * f3;
zm    = satrec->getZmol() + znl * satrec->getT();
if (init == 'y')
    zm = satrec->getZmol();
zf    = zm + 2.0 * zel * sin(zm);
sinzf = sin(zf);
f2    =  0.5 * sinzf * sinzf - 0.25;
f3    = -0.5 * sinzf * cos(zf);
sel   = satrec->getEe2() * f2 + satrec->getE3() * f3;
sil   = satrec->getXi2() * f2 + satrec->getXi3() * f3;
sll   = satrec->getXl2() * f2 + satrec->getXl3() * f3 + satrec->getXl4() * sinzf;
sghl  = satrec->getXgh2() * f2 + satrec->getXgh3() * f3 + satrec->getXgh4() * sinzf;
shll  = satrec->getXh2() * f2 + satrec->getXh3() * f3;
pe    = ses + sel;
pinc  = sis + sil;
pl    = sls + sll;
pgh   = sghs + sghl;
ph    = shs + shll;

if (init == 'n')
  {
  pe    = pe - satrec->getPeo();
  pinc  = pinc - satrec->getPinco();
  pl    = pl - satrec->getPlo();
  pgh   = pgh - satrec->getPgho();
  ph    = ph - satrec->getPho();
  inclp = inclp + pinc;
  ep    = ep + pe;
  sinip = sin(inclp);
  cosip = cos(inclp);

  /* ----------------- apply periodics directly ------------ */
  //  sgp4fix for lyddane choice
  //  strn3 used original inclination - this is technically feasible
  //  gsfc used perturbed inclination - also technically feasible
  //  probably best to readjust the 0.2 limit value and limit discontinuity
  //  0.2 rad = 11.45916 deg
  //  use next line for original strn3 approach and original inclination
  //  if (inclo >= 0.2)
  //  use next line for gsfc version and perturbed inclination
  if (inclp >= 0.2)
    {
      ph     = ph / sinip;
      pgh    = pgh - cosip * ph;
      argpp  = argpp + pgh;
      nodep  = nodep + ph;
      mp     = mp + pl;
    }
    else
    {
      /* ---- apply periodics with lyddane modification ---- */
      sinop  = sin(nodep);
      cosop  = cos(nodep);
      alfdp  = sinip * sinop;
      betdp  = sinip * cosop;
      dalf   =  ph * cosop + pinc * cosip * sinop;
      dbet   = -ph * sinop + pinc * cosip * cosop;
      alfdp  = alfdp + dalf;
      betdp  = betdp + dbet;
      nodep  = fmod(nodep, twopi);
      //  sgp4fix for afspc written intrinsic functions
      // nodep used without a trigonometric function ahead

      if ((nodep < 0.0) && (satrec->getOperationmode() == 'a'))
          nodep = nodep + twopi;
      xls    = mp + argpp + cosip * nodep;
      dls    = pl + pgh - pinc * nodep * sinip;
      xls    = xls + dls;
      xnoh   = nodep;
      nodep  = atan2(alfdp, betdp);
      //  sgp4fix for afspc written intrinsic functions
      // nodep used without a trigonometric function ahead
      if ((nodep < 0.0) && (satrec->getOperationmode() == 'a'))
          nodep = nodep + twopi;
      if (fabs(xnoh - nodep) > pi){
        if (nodep < xnoh)
           nodep = nodep + twopi;
          else
           nodep = nodep - twopi;}
      mp    = mp + pl;
      argpp = xls - mp - cosip * nodep;
    }
  }   // if init == 'n'

//#include "debug1.cpp"
}  // end dpper

/**
 * @brief Esta función aporta los típicos elementos del espacio profundo utilizados por subrutinas seculares y periódicas.
 * @param epoch parámetro de entrada.
 * @param tc parámetro de entrada.
 * @param snodm parámetro de entrada / salida.
 * @param cnodm parámetro de entrada / salida.
 * @param sinim parámetro de entrada / salida.
 * @param cosim parámetro de entrada / salida.
 * @param sinomm parámetro de entrada / salida.
 * @param cosomm parámetro de entrada / salida.
 * @param day parámetro de entrada / salida.
 * @param em parámetro de entrada / salida.
 * @param emsq parámetro de entrada / salida.
 * @param gam parámetro de entrada / salida.
 * @param rtemsq parámetro de entrada / salida.
 * @param s1 parámetro de entrada / salida.
 * @param s2 parámetro de entrada / salida.
 * @param s3 parámetro de entrada / salida.
 * @param s4 parámetro de entrada / salida.
 * @param s5 parámetro de entrada / salida.
 * @param s6 parámetro de entrada / salida.
 * @param s7 parámetro de entrada / salida.
 * @param ss1 parámetro de entrada / salida.
 * @param ss2 parámetro de entrada / salida.
 * @param ss3 parámetro de entrada / salida.
 * @param ss4 parámetro de entrada / salida.
 * @param ss5 parámetro de entrada / salida.
 * @param ss6 parámetro de entrada / salida.
 * @param ss7 parámetro de entrada / salida.
 * @param sz1 parámetro de entrada / salida.
 * @param sz2 parámetro de entrada / salida.
 * @param sz3 parámetro de entrada / salida.
 * @param sz11 parámetro de entrada / salida.
 * @param sz12 parámetro de entrada / salida.
 * @param sz13 parámetro de entrada / salida.
 * @param sz21 parámetro de entrada / salida.
 * @param sz22 parámetro de entrada / salida.
 * @param sz23 parámetro de entrada / salida.
 * @param sz31 parámetro de entrada / salida.
 * @param sz32 parámetro de entrada / salida.
 * @param sz33 parámetro de entrada / salida.
 * @param nm parámetro de entrada / salida.
 * @param z1 parámetro de entrada / salida.
 * @param z2 parámetro de entrada / salida.
 * @param z3 parámetro de entrada / salida.
 * @param z11 parámetro de entrada / salida.
 * @param z12 parámetro de entrada / salida.
 * @param z13 parámetro de entrada / salida.
 * @param z21 parámetro de entrada / salida.
 * @param z22 parámetro de entrada / salida.
 * @param z23 parámetro de entrada / salida.
 * @param z31 parámetro de entrada / salida.
 * @param z32 parámetro de entrada / salida.
 * @param z33 parámetro de entrada / salida.
 */
void Sgp4unit::dscom
     (
       double epoch, double tc,
       double& snodm, double& cnodm, double& sinim,  double& cosim, double& sinomm,
       double& cosomm,double& day,   double& em,
       double& emsq,  double& gam,   double& rtemsq,
       double& s1,    double& s2,    double& s3,     double& s4,    double& s5,
       double& s6,    double& s7,    double& ss1,    double& ss2,   double& ss3,
       double& ss4,   double& ss5,   double& ss6,    double& ss7,   double& sz1,
       double& sz2,   double& sz3,   double& sz11,   double& sz12,  double& sz13,
       double& sz21,  double& sz22,  double& sz23,   double& sz31,  double& sz32,
       double& sz33,  double& nm,    double& z1,     double& z2,    double& z3,
       double& z11,   double& z12,   double& z13,    double& z21,   double& z22,
       double& z23,   double& z31,   double& z32,    double& z33
     )

{
	/*dscom
                 (
                   epoch, tc, snodm, cnodm,  sinim, cosim,sinomm,     cosomm,
                   day,em,         emsq, gam,
                   rtemsq, s1, s2, s3, s4, s5,
                   s6,   s7,   ss1,  ss2,  ss3,  ss4,  ss5,  ss6,  ss7, sz1, sz2, sz3,
                   sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33,
                   nm, z1, z2, z3, z11,
                   z12, z13, z21, z22, z23, z31, z32, z33,
                   
                 );
     /* -------------------------- constants ------------------------- */
     const double zes     =  0.01675;
     const double zel     =  0.05490;
     const double c1ss    =  2.9864797e-6;
     const double c1l     =  4.7968065e-7;
     const double zsinis  =  0.39785416;
     const double zcosis  =  0.91744867;
     const double zcosgs  =  0.1945905;
     const double zsings  = -0.98088458;
     const double twopi   =  2.0 * pi;

     /* --------------------- local variables ------------------------ */
     int lsflg;
     double a1    , a2    , a3    , a4    , a5    , a6    , a7    ,
        a8    , a9    , a10   , betasq, cc    , ctem  , stem  ,
        x1    , x2    , x3    , x4    , x5    , x6    , x7    ,
        x8    , xnodce, xnoi  , zcosg , zcosgl, zcosh , zcoshl,
        zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini ,
        zsinil, zx    , zy;

     nm     = satrec->getNo();
     em     = satrec->getEcco();
     snodm  = sin(satrec->getNodeo());
     cnodm  = cos(satrec->getNodeo());
     sinomm = sin(satrec->getArgpo());
     cosomm = cos(satrec->getArgpo());
     sinim  = sin(satrec->getInclo());
     cosim  = cos(satrec->getInclo());
     emsq   = em * em;
     betasq = 1.0 - emsq;
     rtemsq = sqrt(betasq);

     /* ----------------- initialize lunar solar terms --------------- */
     satrec->setPeo(0.0);
     satrec->setPinco(0.0);
     satrec->setPlo(0.0);
     satrec->setPgho(0.0);
     satrec->setPho(0.0);
     day    = epoch + 18261.5 + tc / 1440.0;
     xnodce = fmod(4.5236020 - 9.2422029e-4 * day, twopi);
     stem   = sin(xnodce);
     ctem   = cos(xnodce);
     zcosil = 0.91375164 - 0.03568096 * ctem;
     zsinil = sqrt(1.0 - zcosil * zcosil);
     zsinhl = 0.089683511 * stem / zsinil;
     zcoshl = sqrt(1.0 - zsinhl * zsinhl);
     gam    = 5.8351514 + 0.0019443680 * day;
     zx     = 0.39785416 * stem / zsinil;
     zy     = zcoshl * ctem + 0.91744867 * zsinhl * stem;
     zx     = atan2(zx, zy);
     zx     = gam + zx - xnodce;
     zcosgl = cos(zx);
     zsingl = sin(zx);

     /* ------------------------- do solar terms --------------------- */
     zcosg = zcosgs;
     zsing = zsings;
     zcosi = zcosis;
     zsini = zsinis;
     zcosh = cnodm;
     zsinh = snodm;
     cc    = c1ss;
     xnoi  = 1.0 / nm;

     for (lsflg = 1; lsflg <= 2; lsflg++)
       {
         a1  =   zcosg * zcosh + zsing * zcosi * zsinh;
         a3  =  -zsing * zcosh + zcosg * zcosi * zsinh;
         a7  =  -zcosg * zsinh + zsing * zcosi * zcosh;
         a8  =   zsing * zsini;
         a9  =   zsing * zsinh + zcosg * zcosi * zcosh;
         a10 =   zcosg * zsini;
         a2  =   cosim * a7 + sinim * a8;
         a4  =   cosim * a9 + sinim * a10;
         a5  =  -sinim * a7 + cosim * a8;
         a6  =  -sinim * a9 + cosim * a10;

         x1  =  a1 * cosomm + a2 * sinomm;
         x2  =  a3 * cosomm + a4 * sinomm;
         x3  = -a1 * sinomm + a2 * cosomm;
         x4  = -a3 * sinomm + a4 * cosomm;
         x5  =  a5 * sinomm;
         x6  =  a6 * sinomm;
         x7  =  a5 * cosomm;
         x8  =  a6 * cosomm;

         z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3;
         z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4;
         z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4;
         z1  =  3.0 *  (a1 * a1 + a2 * a2) + z31 * emsq;
         z2  =  6.0 *  (a1 * a3 + a2 * a4) + z32 * emsq;
         z3  =  3.0 *  (a3 * a3 + a4 * a4) + z33 * emsq;
         z11 = -6.0 * a1 * a5 + emsq *  (-24.0 * x1 * x7-6.0 * x3 * x5);
         z12 = -6.0 *  (a1 * a6 + a3 * a5) + emsq *
                (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5));
         z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6);
         z21 =  6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7);
         z22 =  6.0 *  (a4 * a5 + a2 * a6) + emsq *
                (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8));
         z23 =  6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8);
         z1  = z1 + z1 + betasq * z31;
         z2  = z2 + z2 + betasq * z32;
         z3  = z3 + z3 + betasq * z33;
         s3  = cc * xnoi;
         s2  = -0.5 * s3 / rtemsq;
         s4  = s3 * rtemsq;
         s1  = -15.0 * em * s4;
         s5  = x1 * x3 + x2 * x4;
         s6  = x2 * x3 + x1 * x4;
         s7  = x2 * x4 - x1 * x3;

         /* ----------------------- do lunar terms ------------------- */
         if (lsflg == 1)
           {
             ss1   = s1;
             ss2   = s2;
             ss3   = s3;
             ss4   = s4;
             ss5   = s5;
             ss6   = s6;
             ss7   = s7;
             sz1   = z1;
             sz2   = z2;
             sz3   = z3;
             sz11  = z11;
             sz12  = z12;
             sz13  = z13;
             sz21  = z21;
             sz22  = z22;
             sz23  = z23;
             sz31  = z31;
             sz32  = z32;
             sz33  = z33;
             zcosg = zcosgl;
             zsing = zsingl;
             zcosi = zcosil;
             zsini = zsinil;
             zcosh = zcoshl * cnodm + zsinhl * snodm;
             zsinh = snodm * zcoshl - cnodm * zsinhl;
             cc    = c1l;
          }
       }

     satrec->setZmol(fmod(4.7199672 + 0.22997150  * day - gam, twopi));
     satrec->setZmos(fmod(6.2565837 + 0.017201977 * day, twopi));

     /* ------------------------ do solar terms ---------------------- */
     satrec->setSe2(2.0 * ss1 * ss6);
     satrec->setSe3(2.0 * ss1 * ss7);
     satrec->setSi2(2.0 * ss2 * sz12);
     satrec->setSi3(2.0 * ss2 * (sz13 - sz11));
     satrec->setSl2(-2.0 * ss3 * sz2);
     satrec->setSl3(-2.0 * ss3 * (sz3 - sz1));
     satrec->setSl4(-2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes);
     satrec->setSgh2(2.0 * ss4 * sz32);
     satrec->setSgh3(2.0 * ss4 * (sz33 - sz31));
     satrec->setSgh4(-18.0 * ss4 * zes);
     satrec->setSh2(-2.0 * ss2 * sz22);
     satrec->setSh3(-2.0 * ss2 * (sz23 - sz21));

     /* ------------------------ do lunar terms ---------------------- */
     satrec->setEe2(2.0 * s1 * s6);
     satrec->setE3(2.0 * s1 * s7);
     satrec->setXi2(2.0 * s2 * z12);
     satrec->setXi3(2.0 * s2 * (z13 - z11));
     satrec->setXl2(-2.0 * s3 * z2);
     satrec->setXl3(-2.0 * s3 * (z3 - z1));
     satrec->setXl4(-2.0 * s3 * (-21.0 - 9.0 * emsq) * zel);
     satrec->setXgh2(2.0 * s4 * z32);
     satrec->setXgh3(2.0 * s4 * (z33 - z31));
     satrec->setXgh4(-18.0 * s4 * zel);
     satrec->setXh2(-2.0 * s2 * z22);
     satrec->setXh3(-2.0 * s2 * (z23 - z21));

//#include "debug2.cpp"
}  // end dscom

/**
 * @brief Esta función aporta información del espacio profundo en referencia a elementos del tercer cuerpo perturbadores.
 * @param tc parámetro de entrada.
 * @param em parámetro de entrada / salida.
 * @param argpm parámetro de entrada / salida.
 * @param inclm parámetro de entrada / salida.
 * @param mm parámetro de entrada / salida.
 * @param nodem parámetro de entrada / salida.
 * @param dndt parámetro de entrada / salida.
 * @param nm parámetro de entrada / salida.
 */
void Sgp4unit::dspace
	/*
	 *
               satrec->d2201, satrec->d2211, satrec->d3210,
               satrec->d3222, satrec->d4410, satrec->d4422,
               satrec->d5220, satrec->d5232, satrec->d5421,
               satrec->d5433, satrec->dedt,  satrec->del1,
               satrec->del2,  satrec->del3,  satrec->didt,
               satrec->dmdt,  satrec->dnodt, satrec->domdt,
               satrec->argpo, satrec->argpdot, satrec->t, tc,
               satrec->gsto, satrec->xfact, satrec->xlamo,
               satrec->no, satrec->atime,
               em, argpm, inclm, satrec->xli, mm, satrec->xni,
               nodem, dndt, nm
	 */
     (

    		        double tc,
    		        double& em,    double& argpm,  double& inclm,
    		        double& mm,    double& nodem,  double& dndt,  double& nm
     )
{
     const double twopi = 2.0 * pi;
     int iretn , iret;
     double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32,
          g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp;

     fasx2 = 0.13130908;
     fasx4 = 2.8843198;
     fasx6 = 0.37448087;
     g22   = 5.7686396;
     g32   = 0.95240898;
     g44   = 1.8014998;
     g52   = 1.0508330;
     g54   = 4.4108898;
     rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec
     stepp =    720.0;
     stepn =   -720.0;
     step2 = 259200.0;

     /* ----------- calculate deep space resonance effects ----------- */
     dndt   = 0.0;
     theta  = fmod(satrec->getGsto() + tc * rptim, twopi);
     em     = em + satrec->getDedt() * satrec->getT();

     inclm  = inclm + satrec->getDidt() * satrec->getT();
     argpm  = argpm + satrec->getDomdt() *satrec->getT();
     nodem  = nodem + satrec->getDnodt() * satrec->getT();
     mm     = mm + satrec->getDmdt() * satrec->getT();

     //   sgp4fix for negative inclinations
     //   the following if statement should be commented out
     //  if (inclm < 0.0)
     // {
     //    inclm = -inclm;
     //    argpm = argpm - pi;
     //    nodem = nodem + pi;
     //  }

     /* - update resonances : numerical (euler-maclaurin) integration - */
     /* ------------------------- epoch restart ----------------------  */
     //   sgp4fix for propagator problems
     //   the following integration works for negative time steps and periods
     //   the specific changes are unknown because the original code was so convoluted

     // sgp4fix take out atime = 0.0 and fix for faster operation
     ft    = 0.0;
     if (satrec->getIrez() != 0)
       {
         // sgp4fix streamline check
         if ((satrec->getAtime() == 0.0) || (satrec->getT() * satrec->getAtime() <= 0.0) || (fabs(satrec->getT()) < fabs(satrec->getAtime())) )
           {
        	 satrec->setAtime(0.0);
        	 satrec->setXni(satrec->getNo());
        	 satrec->setXli(satrec->getXlamo());
           }
           // sgp4fix move check outside loop
           if (satrec->getT() > 0.0)
               delt = stepp;
             else
               delt = stepn;

         iretn = 381; // added for do loop
         iret  =   0; // added for loop
         while (iretn == 381)
           {
             /* ------------------- dot terms calculated ------------- */
             /* ----------- near - synchronous resonance terms ------- */
             if (satrec->getIrez() != 2)
               {
                 xndt  = satrec->getDel1() * sin(satrec->getXli() - fasx2) + satrec->getDel2() * sin(2.0 * (satrec->getXli() - fasx4)) +
                		 satrec->getDel3() * sin(3.0 * (satrec->getXli() - fasx6));
                 xldot = satrec->getXni() + satrec->getXfact();
                 xnddt = satrec->getDel1() * cos(satrec->getXli() - fasx2) +
                         2.0 * satrec->getDel2() * cos(2.0 * (satrec->getXli() - fasx4)) +
                         3.0 * satrec->getDel3() * cos(3.0 * (satrec->getXli() - fasx6));
                 xnddt = xnddt * xldot;
               }
               else
               {
                 /* --------- near - half-day resonance terms -------- */
                 xomi  = satrec->getArgpo() + satrec->getArgpdot() * satrec->getAtime();
                 x2omi = xomi + xomi;
                 x2li  = satrec->getXli() + satrec->getXli();
                 xndt  = satrec->getD2201() * sin(x2omi + satrec->getXli() - g22) + satrec->getD2211() * sin(satrec->getXli() - g22) +
                       satrec->getD3210() * sin(xomi + satrec->getXli() - g32)  + satrec->getD3222() * sin(-xomi + satrec->getXli() - g32)+
                       satrec->getD4410() * sin(x2omi + x2li - g44)+ satrec->getD4422() * sin(x2li - g44) +
                       satrec->getD5220() * sin(xomi + satrec->getXli() - g52)  + satrec->getD5232() * sin(-xomi + satrec->getXli() - g52)+
                       satrec->getD5421() * sin(xomi + x2li - g54) + satrec->getD5433() * sin(-xomi + x2li - g54);
                 xldot = satrec->getXni() + satrec->getXfact();
                 xnddt = satrec->getD2201() * cos(x2omi + satrec->getXli() - g22) + satrec->getD2211() * cos(satrec->getXli() - g22) +
                       satrec->getD3210() * cos(xomi + satrec->getXli() - g32) + satrec->getD3222() * cos(-xomi + satrec->getXli() - g32) +
                       satrec->getD5220() * cos(xomi + satrec->getXli() - g52) + satrec->getD5232() * cos(-xomi + satrec->getXli() - g52) +
                       2.0 * (satrec->getD4410() * cos(x2omi + x2li - g44) +
                       satrec->getD4422() * cos(x2li - g44) + satrec->getD5421() * cos(xomi + x2li - g54) +
                       satrec->getD5433() * cos(-xomi + x2li - g54));
                 xnddt = xnddt * xldot;
               }

             /* ----------------------- integrator ------------------- */
             // sgp4fix move end checks to end of routine
             if (fabs(satrec->getT() - satrec->getAtime()) >= stepp)
               {
                 iret  = 0;
                 iretn = 381;
               }
               else // exit here
               {
                 ft    = satrec->getT() - satrec->getAtime();
                 iretn = 0;
               }

             if (iretn == 381)
               {
            	 satrec->setXli(satrec->getXli() + xldot * delt + xndt * step2);
            	 satrec->setXni(satrec->getXni() + xndt * delt + xnddt * step2);
            	 satrec->setAtime(satrec->getAtime() + delt);
               }
           }  // while iretn = 381

         nm = satrec->getXni() + xndt * ft + xnddt * ft * ft * 0.5;
         xl = satrec->getXli() + xldot * ft + xndt * ft * ft * 0.5;
         if (satrec->getIrez() != 1)
           {
             mm   = xl - 2.0 * nodem + 2.0 * theta;
             dndt = nm - satrec->getNo();
           }
           else
           {
             mm   = xl - nodem - argpm + theta;
             dndt = nm - satrec->getNo();
           }
         nm = satrec->getNo() + dndt;
       }

//#include "debug4.cpp"
}  // end dsspace

/*-----------------------------------------------------------------------------
*
*                           procedure initl
*
*  this procedure initializes the spg4 propagator. all the initialization is
*    consolidated here instead of having multiple loops inside other routines.
*
*  author        : david vallado                  719-573-2600   28 jun 2005
*
*  inputs        :
*    ecco        - eccentricity                           0.0 - 1.0
*    epoch       - epoch time in days from jan 0, 1950. 0 hr
*    inclo       - inclination of satellite
*    no          - mean motion of satellite
*    satn        - satellite number
*
*  outputs       :
*    ainv        - 1.0 / a
*    ao          - semi major axis
*    con41       -
*    con42       - 1.0 - 5.0 cos(i)
*    cosio       - cosine of inclination
*    cosio2      - cosio squared
*    eccsq       - eccentricity squared
*    method      - flag for deep space                    'd', 'n'
*    omeosq      - 1.0 - ecco * ecco
*    posq        - semi-parameter squared
*    rp          - radius of perigee
*    rteosq      - square root of (1.0 - ecco*ecco)
*    sinio       - sine of inclination
*    gsto        - gst at time of observation               rad
*    no          - mean motion of satellite
*
*  locals        :
*    ak          -
*    d1          -
*    del         -
*    adel        -
*    po          -
*
*  coupling      :
*    getgravconst
*    gstime      - find greenwich sidereal time from the julian date
*
*  references    :
*    hoots, roehrich, norad spacetrack report #3 1980
*    hoots, norad spacetrack report #6 1986
*    hoots, schumacher and glover 2004
*    vallado, crawford, hujsak, kelso  2006
  ----------------------------------------------------------------------------*/

/**
 * Esta función inicializa el propagador de sgp4.
 * @param satn parámetro de entrada.
 * @param whichconst Conjunto de constantes gravitacionales.
 * @param epoch parámetro de entrada.
 * @param ainv parámetro de entrada / salida.
 * @param ao parámetro de entrada / salida.
 * @param con42 parámetro de entrada / salida.
 * @param cosio parámetro de entrada / salida.
 * @param cosio2 parámetro de entrada / salida.
 * @param eccsq parámetro de entrada / salida.
 * @param omeosq parámetro de entrada / salida.
 * @param posq parámetro de entrada / salida.
 * @param rp parámetro de entrada / salida.
 * @param rteosq parámetro de entrada / salida.
 * @param sinio parámetro de entrada / salida.
 */
void Sgp4unit::initl
     (
       int satn,      gravconsttype whichconst,
       double epoch,
       double& ainv,  double& ao,  double& con42, double& cosio,
       double& cosio2,double& eccsq, double& omeosq, double& posq,
       double& rp,    double& rteosq,double& sinio)
{
     /* --------------------- local variables ------------------------ */
     double ak, d1, del, adel, po, x2o3, j2, xke,
            tumin, mu, radiusearthkm, j3, j4, j3oj2;

     // sgp4fix use old way of finding gst
     double ds70;
     double ts70, tfrac, c1, thgr70, fk5r, c1p2p;//, thgr, thgro;
     const double twopi = 2.0 * pi;

     /* ----------------------- earth constants ---------------------- */
     // sgp4fix identify constants and allow alternate values
     getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
     x2o3   = 2.0 / 3.0;

     /* ------------- calculate auxillary epoch quantities ---------- */
     eccsq  = satrec->getEcco() * satrec->getEcco();
     omeosq = 1.0 - eccsq;
     rteosq = sqrt(omeosq);
     cosio  = cos(satrec->getInclo());
     cosio2 = cosio * cosio;

     /* ------------------ un-kozai the mean motion ----------------- */

     ak    = pow(xke / satrec->getNo(), x2o3);
     d1    = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq);
     del   = d1 / (ak * ak);
     adel  = ak * (1.0 - del * del - del *
             (1.0 / 3.0 + 134.0 * del * del / 81.0));
     del   = d1/(adel * adel);
     satrec->setNo(satrec->getNo() / (1.0 + del));

     ao    = pow(xke / satrec->getNo(), x2o3);
     sinio = sin(satrec->getInclo());
     po    = ao * omeosq;
     con42 = 1.0 - 5.0 * cosio2;
     satrec->setCon41(-con42-cosio2-cosio2);
     ainv  = 1.0 / ao;
     posq  = po * po;
     rp    = ao * (1.0 - satrec->getEcco());
     satrec->setMethod('n');

     // sgp4fix modern approach to finding sidereal time
     if (satrec->getOperationmode() == 'a')
        {
         // sgp4fix use old way of finding gst
         // count integer number of days from 0 jan 1970
         ts70  = epoch - 7305.0;
         ds70 = floor(ts70 + 1.0e-8);
         tfrac = ts70 - ds70;
         // find greenwich location at epoch
         c1    = 1.72027916940703639e-2;
         thgr70= 1.7321343856509374;
         fk5r  = 5.07551419432269442e-15;
         c1p2p = c1 + twopi;
         satrec->setGsto(fmod( thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi));
         if ( satrec->getGsto() < 0.0 )
        	 satrec->setGsto(satrec->getGsto() + twopi);
       }
       else

    	   satrec->setGsto(sgp4_date::gstime(epoch + 2433281.5));


//#include "debug5.cpp"
}  // end initl

/**
 * @brief Inicializa las variables del sistema sgp4.
 * @param whichconst Conjunto de constantes gravitacionales.
 * @param opsmode Tipo de operación. 'a' para afspc, 'i' para mejora.
 * @return true si se ha inicializado correctamente. En el caso contrario devuelve false.
 */
bool Sgp4unit::sgp4init
     (
       gravconsttype whichconst, char opsmode
     )
{
     /* --------------------- local variables ------------------------ */
     double ao, ainv,   con42, cosio, sinio, cosio2, eccsq,
          omeosq, posq,   rp,     rteosq,
          cnodm , snodm , cosim , sinim , cosomm, sinomm, cc1sq ,
          cc2   , cc3   , coef  , coef1 , cosio4, day   , dndt  ,
          em    , emsq  , eeta  , etasq , gam   , argpm , nodem ,
          inclm , mm    , nm    , perige, pinvsq, psisq , qzms24,
          rtemsq, s1    , s2    , s3    , s4    , s5    , s6    ,
          s7    , sfour , ss1   , ss2   , ss3   , ss4   , ss5   ,
          ss6   , ss7   , sz1   , sz2   , sz3   , sz11  , sz12  ,
          sz13  , sz21  , sz22  , sz23  , sz31  , sz32  , sz33  ,
          tc    , temp  , temp1 , temp2 , temp3 , tsi   , xpidot,
          xhdot1, z1    , z2    , z3    , z11   , z12   , z13   ,
          z21   , z22   , z23   , z31   , z32   , z33,
          qzms2t, ss, j2, j3oj2, j4, x2o3, r[3], v[3],
          tumin, mu, radiusearthkm, xke, j3;

     /* ------------------------ initialization --------------------- */
     // sgp4fix divisor for divide by zero check on inclination
     // the old check used 1.0 + cos(pi-1.0e-9), but then compared it to
     // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency
     const double temp4    =   1.5e-12;
     const int satn = satrec->getSatnum();
     const double epoch =satrec->getJdsatepoch()-2433281.5;

     /* ----------- set all near earth variables to zero ------------ */

     // sgp4fix - note the following variables are also passed directly via satrec->
     // it is possible to streamline the sgp4init call by deleting the "x"
     // variables, but the user would need to set the satrec->* values first. we
     // include the additional assignments in case twoline2rv is not used.


     // sgp4fix add opsmode
     satrec->setOperationmode(opsmode);

     /* ------------------------ earth constants ----------------------- */
     // sgp4fix identify constants and allow alternate values
     getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
     ss     = 78.0 / radiusearthkm + 1.0;
     qzms2t = pow(((120.0 - 78.0) / radiusearthkm), 4);
     x2o3   =  2.0 / 3.0;

     satrec->setInit('y');
     satrec->setT(0.0);
     /*
      * void Sgp4unit::initl
     (
       int satn,      gravconsttype whichconst,
       double ecco,   double epoch,  double inclo,   double& no,
       char& method,
       double& ainv,  double& ao,    double& con41,  double& con42, double& cosio,
       double& cosio2,double& eccsq, double& omeosq, double& posq,
       double& rp,    double& rteosq,double& sinio , double& gsto,
       char opsmode
     )
      *
      */
     initl
         (
           satn, whichconst, epoch,
           ainv, ao, con42, cosio, cosio2, eccsq, omeosq,
           posq, rp, rteosq, sinio
         );
     satrec->setError(0);

     // sgp4fix remove this check as it is unnecessary
     // the mrt check in sgp4 handles decaying satellite cases even if the starting
     // condition is below the surface of te earth
//     if (rp < 1.0)
//       {
//         printf("# *** satn%d epoch elts sub-orbital ***\n", satn);
//         satrec->error = 5;
//       }

     if ((omeosq >= 0.0 ) || ( satrec->getNo() >= 0.0))
       {
         satrec->setIsimp(0);
         if (rp < (220.0 / radiusearthkm + 1.0))
        	 satrec->setIsimp(1);
         sfour  = ss;
         qzms24 = qzms2t;
         perige = (rp - 1.0) * radiusearthkm;

         /* - for perigees below 156 km, s and qoms2t are altered - */
         if (perige < 156.0)
           {
             sfour = perige - 78.0;
             if (perige < 98.0)
                 sfour = 20.0;
             qzms24 = pow(((120.0 - sfour) / radiusearthkm), 4.0);
             sfour  = sfour / radiusearthkm + 1.0;
           }
         pinvsq = 1.0 / posq;

         tsi  = 1.0 / (ao - sfour);
         satrec->setEta(ao * satrec->getEcco() * tsi);
         etasq = satrec->getEta() * satrec->getEta();
         eeta  = satrec->getEcco() * satrec->getEta();
         psisq = fabs(1.0 - etasq);
         coef  = qzms24 * pow(tsi, 4.0);
         coef1 = coef / pow(psisq, 3.5);
         cc2   = coef1 * satrec->getNo() * (ao * (1.0 + 1.5 * etasq + eeta *
                        (4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec->getCon41() *
                        (8.0 + 3.0 * etasq * (8.0 + etasq)));
         satrec->setCc1(satrec->getBstar() * cc2);
         cc3   = 0.0;
         if (satrec->getEcco() > 1.0e-4)
             cc3 = -2.0 * coef * tsi * j3oj2 * satrec->getNo() * sinio / satrec->getEcco();
         satrec->setX1mth2(1.0 - cosio2);
         satrec->setCc4(2.0* satrec->getNo() * coef1 * ao * omeosq * (satrec->getEta() * (2.0 + 0.5 * etasq) + satrec->getEcco() *
                           (0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) *
                           (-3.0 * satrec->getCon41() * (1.0 - 2.0 * eeta + etasq *
                           (1.5 - 0.5 * eeta)) + 0.75 * satrec->getX1mth2() *
                           (2.0 * etasq - eeta * (1.0 + etasq)) * cos(2.0 * satrec->getArgpo()))));
         satrec->setCc5(2.0 * coef1 * ao * omeosq * (1.0 + 2.75 *
                        (etasq + eeta) + eeta * etasq));
         cosio4 = cosio2 * cosio2;
         temp1  = 1.5 * j2 * pinvsq * satrec->getNo();
         temp2  = 0.5 * temp1 * j2 * pinvsq;
         temp3  = -0.46875 * j4 * pinvsq * pinvsq * satrec->getNo();
         satrec->setMdot (satrec->getNo() + 0.5 * temp1 * rteosq * satrec->getCon41() + 0.0625 *
                            temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4));
         satrec->setArgpdot(-0.5 * temp1 * con42 + 0.0625 * temp2 *
                             (7.0 - 114.0 * cosio2 + 395.0 * cosio4) +
                             temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4));
         xhdot1            = -temp1 * cosio;
         satrec->setNodedot(xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) +
                              2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio);
         xpidot            =  satrec->getArgpdot()+ satrec->getNodedot();
         satrec->setOmgcof(satrec->getBstar() * cc3 * cos(satrec->getArgpo()));
         satrec->setXmcof(0.0);
         if (satrec->getEcco() > 1.0e-4)
             satrec->setXmcof(-x2o3 * coef * satrec->getBstar() / eeta);
         satrec->setNodecf(3.5 * omeosq * xhdot1 * satrec->getCc1());
         satrec->setT2cof(1.5 * satrec->getCc1());
         // sgp4fix for divide by zero with xinco = 180 deg
         if (fabs(cosio+1.0) > 1.5e-12)
             satrec->setXlcof(-0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio));
           else
             satrec->setXlcof(-0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4);
         satrec->setAycof(-0.5 * j3oj2 * sinio);
         satrec->setDelmo(pow((1.0 + satrec->getEta() * cos(satrec->getMo())), 3));
         satrec->setSinmao(sin(satrec->getMo()));
         satrec->setX7thm1(7.0 * cosio2 - 1.0);

         /* --------------- deep space initialization ------------- */
         if ((2*pi / satrec->getNo()) >= 225.0)
           {
             satrec->setMethod('d');
             satrec->setIsimp(1);
             tc    =  0.0;
             inclm = satrec->getInclo();

             dscom(
             epoch, tc, snodm, cnodm,  sinim, cosim,sinomm,     cosomm,
                               day,em,         emsq, gam,
                               rtemsq, s1, s2, s3, s4, s5,
                               s6,   s7,   ss1,  ss2,  ss3,  ss4,  ss5,  ss6,  ss7, sz1, sz2, sz3,
                               sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33,
                               nm, z1, z2, z3, z11,
                               z12, z13, z21, z22, z23, z31, z32, z33);
             /*
              * (
       double e3,     double ee2,    double peo,     double pgho,   double pho,
       double pinco,  double plo,    double se2,     double se3,    double sgh2,
       double sgh3,   double sgh4,   double sh2,     double sh3,    double si2,
       double si3,    double sl2,    double sl3,     double sl4,    double t,
       double xgh2,   double xgh3,   double xgh4,    double xh2,    double xh3,
       double xi2,    double xi3,    double xl2,     double xl3,    double xl4,
       double zmol,   double zmos,   double inclo,
       char init,
       double& ep,    double& inclp, double& nodep,  double& argpp, double& mp,
       char opsmode
     )
              */
             double ep,iclp,nodep,argpp,mp;
             ep=satrec->getEcco();
             iclp=satrec->getInclo();
             nodep=satrec->getNodeo();
             argpp=satrec->getArgpo();
             mp=satrec->getMo();
             dpper
                              ( inclm, satrec->getInit(),
                            		  ep,iclp,nodep,argpp,mp

                              );
             satrec->setEcco(ep);
             satrec->setInclo(iclp);
             satrec->setNodeo(nodep);
             satrec->setArgpo(argpp);
             satrec->setMo(mp);
             argpm  = 0.0;
             nodem  = 0.0;
             mm     = 0.0;

             dsinit
                 (
                		 whichconst,
                		cosim, emsq,s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4,
                		ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, tc,
                		xpidot, z1, z3, z11, z13, z21, z23, z31, z33,
                		eccsq, em, argpm, inclm, mm, nm, nodem,
                		dndt
                 );
           }

       /* ----------- set variables if not deep space ----------- */
       if (satrec->getIsimp() != 1)
         {
           cc1sq          = satrec->getCc1() * satrec->getCc1();
           satrec->setD2(4.0 * ao * tsi * cc1sq);
           temp           = satrec->getD2() * tsi * satrec->getCc1() / 3.0;
           satrec->setD3((17.0 * ao + sfour) * temp);
           satrec->setD4(0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) *
                            satrec->getCc1());
           satrec->setT3cof(satrec->getD2() + 2.0 * cc1sq);
           satrec->setT4cof(0.25 * (3.0 * satrec->getD3() + satrec->getCc1() *
                            (12.0 * satrec->getD2() + 10.0 * cc1sq)));
           satrec->setT5cof(0.2 * (3.0 * satrec->getD4() +
                            12.0 * satrec->getCc1() * satrec->getD3() +
                            6.0 * satrec->getD2() * satrec->getD2() +
                            15.0 * cc1sq * (2.0 * satrec->getD2() + cc1sq)));
         }
       } // if omeosq = 0 ...

       /* finally propogate to zero epoch to initialize all others. */
       // sgp4fix take out check to let satellites process until they are actually below earth surface
//       if(satrec->error == 0)
       sgp4(whichconst,0.0, r, v);

       satrec->setInit('n');

//#include "debug6.cpp"
       //sgp4fix return boolean. satrec->error contains any error codes
       return true;
}  // end sgp

/**
 * Esta función ejecuta el sistema sgp4 siguiendo el modelo de predicción del comando espacial.
 * @param whichconst Tipo de conjunto de constantes gravitacionales.
 * @param tsince tiempo que ha pasado en minutos desde el 1-1-1950 a las 0 horas.
 * @param r Vector de posición.
 * @param v Vector de velocidad.
 * @return true si se ha ejecutado correctamente. En caso contrario devuelve false.
 */
bool Sgp4unit::sgp4
     (
       gravconsttype whichconst,  double tsince,
       double r[3],  double v[3]
     )
{
     double am   , axnl  , aynl , betal ,  cosim , cnod  ,
         cos2u, coseo1, cosi , cosip ,  cosisq, cossu , cosu,
         delm , delomg, em   , emsq  ,  ecose , el2   , eo1 ,
         ep   , esine , argpm, argpp ,  argpdf, pl,     mrt = 0.0,
         mvt  , rdotl , rl   , rvdot ,  rvdotl, sinim ,
         sin2u, sineo1, sini , sinip ,  sinsu , sinu  ,
         snod , su    , t2   , t3    ,  t4    , tem5  , temp,
         temp1, temp2 , tempa, tempe ,  templ , u     , ux  ,
         uy   , uz    , vx   , vy    ,  vz    , inclm , mm  ,
         nm   , nodem, xinc , xincp ,  xl    , xlm   , mp  ,
         xmdf , xmx   , xmy  , nodedf, xnode , nodep, tc  , dndt,
         twopi, x2o3  , j2   , j3    , tumin, j4 , xke   , j3oj2, radiusearthkm,
         mu, vkmpersec;
     int ktr;

     /* ------------------ set mathematical constants --------------- */
     // sgp4fix divisor for divide by zero check on inclination
     // the old check used 1.0 + cos(pi-1.0e-9), but then compared it to
     // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency
     const double temp4 =   1.5e-12;
     twopi = 2.0 * pi;
     x2o3  = 2.0 / 3.0;
     // sgp4fix identify constants and allow alternate values
     getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
     vkmpersec     = radiusearthkm * xke/60.0;

     /* --------------------- clear sgp4 error flag ----------------- */
     satrec->setT(tsince);
     satrec->setError(0);

     /* ------- update for secular gravity and atmospheric drag ----- */
     xmdf    = satrec->getMo() + satrec->getMdot() * satrec->getT();
     argpdf  = satrec->getArgpo() + satrec->getArgpdot() * satrec->getT();
     nodedf  = satrec->getNodeo() + satrec->getNodedot() * satrec->getT();
     argpm   = argpdf;
     mm      = xmdf;
     t2      = satrec->getT() * satrec->getT();
     nodem   = nodedf + satrec->getNodecf() * t2;
     tempa   = 1.0 - satrec->getCc1() * satrec->getT();
     tempe   = satrec->getBstar() * satrec->getCc4() * satrec->getT();
     templ   = satrec->getT2cof() * t2;

     if (satrec->getIsimp() != 1)
       {
         delomg = satrec->getOmgcof() * satrec->getT();
         delm   = satrec->getXmcof() *
                  (pow((1.0 + satrec->getEta() * cos(xmdf)), 3) -
                  satrec->getDelmo());
         temp   = delomg + delm;
         mm     = xmdf + temp;
         argpm  = argpdf - temp;
         t3     = t2 * satrec->getT();
         t4     = t3 * satrec->getT();
         tempa  = tempa - satrec->getD2() * t2 - satrec->getD3() * t3 -
                          satrec->getD4() * t4;
         tempe  = tempe + satrec->getBstar() * satrec->getCc5() * (sin(mm) -
                          satrec->getSinmao());
         templ  = templ + satrec->getT3cof() * t3 + t4 * (satrec->getT4cof() +
                          satrec->getT() * satrec->getT5cof());
       }

     nm    = satrec->getNo();
     em    = satrec->getEcco();
     inclm = satrec->getInclo();
     if (satrec->getMethod() == 'd')
       {
         tc = satrec->getT();
         dspace
             (
               tc,
               em, argpm, inclm, mm,
               nodem, dndt, nm
             );
       } // if method = d

     if (nm <= 0.0)
       {
//         printf("# error nm %f\n", nm);
         satrec->setError(2);
         // sgp4fix add return
         return false;
       }
     am = pow((xke / nm),x2o3) * tempa * tempa;
     nm = xke / pow(am, 1.5);
     em = em - tempe;

     // fix tolerance for error recognition
     // sgp4fix am is fixed from the previous nm check
     if ((em >= 1.0) || (em < -0.001)/* || (am < 0.95)*/ )
       {
//         printf("# error em %f\n", em);
         satrec->setError(1);
         // sgp4fix to return if there is an error in eccentricity
         return false;
       }
     // sgp4fix fix tolerance to avoid a divide by zero
     if (em < 1.0e-6)
         em  = 1.0e-6;
     mm     = mm + satrec->getNo() * templ;
     xlm    = mm + argpm + nodem;
     emsq   = em * em;
     temp   = 1.0 - emsq;

     nodem  = fmod(nodem, twopi);
     argpm  = fmod(argpm, twopi);
     xlm    = fmod(xlm, twopi);
     mm     = fmod(xlm - argpm - nodem, twopi);

     /* ----------------- compute extra mean quantities ------------- */
     sinim = sin(inclm);
     cosim = cos(inclm);

     /* -------------------- add lunar-solar periodics -------------- */
     ep     = em;
     xincp  = inclm;
     argpp  = argpm;
     nodep  = nodem;
     mp     = mm;
     sinip  = sinim;
     cosip  = cosim;
     if (satrec->getMethod() == 'd')
       {
         dpper
             (
               satrec->getInclo(),
               'n', ep, xincp, nodep, argpp, mp
             );
         if (xincp < 0.0)
           {
             xincp  = -xincp;
             nodep = nodep + pi;
             argpp  = argpp - pi;
           }
         if ((ep < 0.0 ) || ( ep > 1.0))
           {
//            printf("# error ep %f\n", ep);
             satrec->setError(3);
             // sgp4fix add return
             return false;
           }
       } // if method = d

     /* -------------------- long period periodics ------------------ */
     if (satrec->getMethod() == 'd')
       {
         sinip =  sin(xincp);
         cosip =  cos(xincp);
         satrec->setAycof(-0.5*j3oj2*sinip);
         // sgp4fix for divide by zero for xincp = 180 deg
         if (fabs(cosip+1.0) > 1.5e-12)
             satrec->setXlcof(-0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip));
           else
             satrec->setXlcof(  -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4);
       }
     axnl = ep * cos(argpp);
     temp = 1.0 / (am * (1.0 - ep * ep));
     aynl = ep* sin(argpp) + temp * satrec->getAycof();
     xl   = mp + argpp + nodep + temp * satrec->getXlcof() * axnl;

     /* --------------------- solve kepler's equation --------------- */
     u    = fmod(xl - nodep, twopi);
     eo1  = u;
     tem5 = 9999.9;
     ktr = 1;
     //   sgp4fix for kepler iteration
     //   the following iteration needs better limits on corrections
     while (( fabs(tem5) >= 1.0e-12) && (ktr <= 10) )
       {
         sineo1 = sin(eo1);
         coseo1 = cos(eo1);
         tem5   = 1.0 - coseo1 * axnl - sineo1 * aynl;
         tem5   = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5;
         if(fabs(tem5) >= 0.95)
             tem5 = tem5 > 0.0 ? 0.95 : -0.95;
         eo1    = eo1 + tem5;
         ktr = ktr + 1;
       }

     /* ------------- short period preliminary quantities ----------- */
     ecose = axnl*coseo1 + aynl*sineo1;
     esine = axnl*sineo1 - aynl*coseo1;
     el2   = axnl*axnl + aynl*aynl;
     pl    = am*(1.0-el2);
     if (pl < 0.0)
       {
//         printf("# error pl %f\n", pl);
         satrec->setError(4);
         // sgp4fix add return
         return false;
       }
       else
       {
         rl     = am * (1.0 - ecose);
         rdotl  = sqrt(am) * esine/rl;
         rvdotl = sqrt(pl) / rl;
         betal  = sqrt(1.0 - el2);
         temp   = esine / (1.0 + betal);
         sinu   = am / rl * (sineo1 - aynl - axnl * temp);
         cosu   = am / rl * (coseo1 - axnl + aynl * temp);
         su     = atan2(sinu, cosu);
         sin2u  = (cosu + cosu) * sinu;
         cos2u  = 1.0 - 2.0 * sinu * sinu;
         temp   = 1.0 / pl;
         temp1  = 0.5 * j2 * temp;
         temp2  = temp1 * temp;

         /* -------------- update for short period periodics ------------ */
         if (satrec->getMethod() == 'd')
           {
             cosisq                 = cosip * cosip;
             satrec->setCon41(3.0*cosisq - 1.0);
             satrec->setX1mth2(1.0 - cosisq);
             satrec->setX7thm1(7.0*cosisq - 1.0);
           }
         mrt   = rl * (1.0 - 1.5 * temp2 * betal * satrec->getCon41()) +
                 0.5 * temp1 * satrec->getX1mth2() * cos2u;
         su    = su - 0.25 * temp2 * satrec->getX7thm1() * sin2u;
         xnode = nodep + 1.5 * temp2 * cosip * sin2u;
         xinc  = xincp + 1.5 * temp2 * cosip * sinip * cos2u;
         mvt   = rdotl - nm * temp1 * satrec->getX1mth2() * sin2u / xke;
         rvdot = rvdotl + nm * temp1 * (satrec->getX1mth2() * cos2u +
                 1.5 * satrec->getCon41()) / xke;

         /* --------------------- orientation vectors ------------------- */
         sinsu =  sin(su);
         cossu =  cos(su);
         snod  =  sin(xnode);
         cnod  =  cos(xnode);
         sini  =  sin(xinc);
         cosi  =  cos(xinc);
         xmx   = -snod * cosi;
         xmy   =  cnod * cosi;
         ux    =  xmx * sinsu + cnod * cossu;
         uy    =  xmy * sinsu + snod * cossu;
         uz    =  sini * sinsu;
         vx    =  xmx * cossu - cnod * sinsu;
         vy    =  xmy * cossu - snod * sinsu;
         vz    =  sini * cossu;

         /* --------- position and velocity (in km and km/sec) ---------- */
         r[0] = (mrt * ux)* radiusearthkm;
         r[1] = (mrt * uy)* radiusearthkm;
         r[2] = (mrt * uz)* radiusearthkm;
         v[0] = (mvt * ux + rvdot * vx) * vkmpersec;
         v[1] = (mvt * uy + rvdot * vy) * vkmpersec;
         v[2] = (mvt * uz + rvdot * vz) * vkmpersec;
       }  // if pl > 0

     // sgp4fix for decaying satellites
     if (mrt < 1.0)
       {
//         printf("# decay condition %11.6f \n",mrt);
         satrec->setError(6);
         return false;
       }

//#include "debug7.cpp"
     return true;
}  // end sgp4

